﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;

namespace NIP.ControllerInterface
{
    public class UsbcController :  USBC, IUsbc
    {
        #region Attributes

        [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public delegate void InitializationDelegate(Structs.ConfigData configData);
        [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public delegate void ErrorDelegate(Structs.ErrorInfo errorInfo);
        [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public delegate void HomingDelegate(ref byte b);

        private InitializationDelegate initPtr;
        private ErrorDelegate errorPtr;
        private HomingDelegate homePtr;

        private Structs.ConfigData configData;
        private Structs.ErrorInfo errorInfo;
        private IntPtr configDataPtr;
        private IntPtr errorInfoPtr;

        private short percent;
        private short mm;

        private int[] encPos = new int[8];
        private int[] jointPos = new int[8];
        private int[] xyz = new int[8];

        private const string PATH = "USBC.dll";

        #endregion
        #region Callbacks
        /// <summary>
        /// Initialization Complete callback.
        /// </summary>
        private InitializationCompleteCallback initializationComplete;

        /// <summary>
        /// Error callback.
        /// </summary>
        private ErrorCallback error;

        /// <summary>
        /// Motion Started callback.
        /// </summary>
        private MovementCallback motionStarted;

        /// <summary>
        /// Motion Ended callback.
        /// </summary>
        private MovementCallback motionEnded;

        /// <summary>
        /// Control Status Changed callback.
        /// </summary>
        private WatchControlCallback controlStatusChanged;

        /// <summary>
        /// Homing Status Changed callback.
        /// </summary>
        private HomingNotifierCallback homingStatusChanged;

        /// <summary>
        /// Join Status Changed callback.
        /// </summary>
        private WatchJointCallback jointStatusChanged;

        /// <summary>
        /// Digital Input Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalInputReceived;

        /// <summary>
        /// Digital Output Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalOutputReceived;
        #endregion

        public UsbcController()
        {
            InitializeStructsAndDelegates();
            this.initializationComplete = new InitializationCompleteCallback(OnInitializationCompleted);
            this.error = new ErrorCallback(OnError);
            this.motionStarted = new MovementCallback(OnMotionStarted);
            this.motionEnded = new MovementCallback(OnMotionEnded);
            this.controlStatusChanged = new WatchControlCallback(OnControlStatusChanged);
            this.homingStatusChanged = new HomingNotifierCallback(OnHomingStatusChanged);
            this.jointStatusChanged = new WatchJointCallback(OnJointStatusChanged);
            this.digitalInputReceived = new IOMonitorCallback(OnDigitalInputReceived);
            this.digitalOutputReceived = new IOMonitorCallback(OnDigitalOutputReceived);

        }
        //===============================================================================================
        // Structs (ErrorInfo, ConfigData), Delegates and CallBackFunctions
        //===============================================================================================
        #region Structs, Delegates and CallBacks

        public void InitializeStructsAndDelegates()
        {
            InitializeStructs();
            InitializeCallBackDelegates();
        }

        private void InitializeStructs()
        {
            InitializeConfigData();
            InitializeErrorInfo();
        }

        private void InitializeConfigData()
        {
            configData = new Structs.ConfigData();
            configDataPtr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(Structs.ConfigData)));
            Marshal.StructureToPtr(configData, configDataPtr, false);
        }

        private void InitializeErrorInfo()
        {
            errorInfo = new Structs.ErrorInfo();
            errorInfoPtr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(Structs.ErrorInfo)));
            Marshal.StructureToPtr(errorInfo, errorInfoPtr, false);
        }

        public void ReleaseStructs()
        {
            Marshal.FreeHGlobal(configDataPtr);
            Marshal.FreeHGlobal(errorInfoPtr);
        }

        private void InitializeCallBackDelegates()
        {
            initPtr = new InitializationDelegate(InitializationCallBack);
            errorPtr = new ErrorDelegate(ErrorCallBack);
            homePtr = new HomingDelegate(HomingCallBack);
        }

        public delegate void StatusEventHandler(object sender, StatusEventArgs se);
        public event StatusEventHandler StatusEvent;

        private void InitializationCallBack(Structs.ConfigData configData)
        {
            StatusEventArgs statusArgs = new StatusEventArgs("Initialized!");
            if (StatusEvent != null)
                StatusEvent(this, statusArgs);
        }

        private void ErrorCallBack(Structs.ErrorInfo errorInfo)
        {
            StatusEventArgs statusArgs = new StatusEventArgs(ConvertError(errorInfo.lNumber));
            if (StatusEvent != null)
                StatusEvent(this, statusArgs);
        }

        private void HomingCallBack(ref byte b)
        {
            StatusEventArgs statusArgs = new StatusEventArgs(ConvertHoming(b));
            if (StatusEvent != null)
                StatusEvent(this, statusArgs);
        }

        private string ConvertError(Int32 errorCode)
        {
            switch (errorCode)
            {
                // put error number in case and string describtion in return
                case 20: return "Controller is not responding";
                case 201: return "Position error during the motion is grater then allowed or impact has occured";
                case 300: return "Emergency ON!!";
                case 301: return "Emergency OFF!!";
                case 533: return "Invalid parameter";
                case 561: return "Time for homing elapsed";
                case 569: return "Homing aborted by user";
                case 586: return "Invalid motion duration";
                case 903: return "Control disabled";
                case 907: return "Given point is not in the workspace of the joints";
                case 908: return "Given point is not in the workspace of the Cartesian space";
                case 911: return "Motion in progress";
                case 912: return "Configuration change";
                default: return "Unknown error: " + errorCode;
            }
        }

        private string ConvertHoming(long code)
        {
            switch (code)
            {
                case 1: return "Homing axis 1";
                case 2: return "Homing axis 2";
                case 3: return "Homing axis 3";
                case 4: return "Homing axis 4";
                case 5: return "Homing axis 5";
                case 6: return "Homing axis 6";
                case 7: return "Homing axis 7";
                case 8: return "Homing axis 8";
                case 64: return "Homing ended";
                case 255: return "Homing started";
                default: return "Unknown response: " + code;
            }
        }

        #endregion

        //===============================================================================================
        // Setup
        //===============================================================================================
        #region Setup functions

        //Initialization
        /// <summary>
        /// Initializes the connection between the USB-controller and the robot
        /// </summary>
        /// <param name="sMode">
        ///     Selects initialization mode:
        ///         0: selects last used mode (from .ini file)
        ///         1: force online mode
        ///         2: simulation mode
        /// </param>
        /// <param name="sSystemType">
        ///     Selects system type:
        ///         0: let the library detect the robot
        ///         41: define robot type as ER-4 Scorbot with USB-connection
        /// </param>
        /// <returns>Returns true if the function has been succesfully run, otherwise false</returns>
        public bool Initialization(short sMode, short sSystemType)
        {
            //return Wrapper.Initialization(sMode, sSystemType, initPtr, errorPtr);
            if (initializationComplete == null || error == null)
            {
                throw new InvalidOperationException();
            }

            var initialized = Wrapper.Initialization((short)sMode,
                (short)sSystemType, initializationComplete, error);

            if (initialized)
            {
                MonitorDigitalInput();
                MonitorDigitalOutput();
                MonitorControlStatus();
                MonitorHomingStatus();
                MonitorJointStatus();
                MonitorMotion();
            }

            return initialized;

        }

        //Control
        /// <summary>
        /// Turns control on or off for a specific axis group
        /// </summary>
        /// <param name="ucGroupAxis">
        ///     Selects which axis to control:
        ///         'A': robot
        ///         'B': peripherals
        ///         '&': all axes 
        /// </param>
        /// <param name="bIsOn">
        ///     true: control on
        ///     false control off
        /// </param>
        /// <returns></returns>
        public bool Control(byte ucGroupAxis, bool bIsOn)
        {
            StatusEventArgs statusArgs;
            bool b = Wrapper.Control(ucGroupAxis, bIsOn);
            if (b == true)
            {
                if (bIsOn == true)
                {
                    statusArgs = new StatusEventArgs("Control is ON");
                    if (StatusEvent != null)
                        StatusEvent(this, statusArgs);
                }
                else
                {
                    statusArgs = new StatusEventArgs("Control is OFF");
                    if (StatusEvent != null)
                        StatusEvent(this, statusArgs);
                }
            }
            else
            {
                statusArgs = new StatusEventArgs("Control function not properly run!");
                if (StatusEvent != null)
                    StatusEvent(this, statusArgs);
            }

            return b;
        }

        //Home
        /// <summary>
        /// Homes the robot
        /// </summary>
        /// <param name="ucGroupAxis">
        ///     Selects the axis to home:
        ///         
        /// </param>
        /// <returns></returns>
        public bool Home(byte ucGroupAxis)
        {
            return Wrapper.Home(ucGroupAxis, homePtr);
        }

        #endregion

        //===============================================================================================
        // Movement
        //===============================================================================================
        #region Movement functions
        #region Monitor
        /// <summary>
        /// Monitors the robots movement.
        /// </summary>
        public void MonitorMotion()
        {
            if (motionStarted == null || motionEnded == null)
            {
                throw new InvalidOperationException();
            }

            Wrapper.WatchMotion(motionEnded, motionStarted);
        }

        /// <summary>
        /// Monitors the digital input.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalInput()
        {
            if (digitalInputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return Wrapper.BeginMonitorDigitalInput(digitalInputReceived);
        }

        /// <summary>
        /// Monitors the digital output.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalOutput()
        {
            if (digitalOutputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return Wrapper.BeginMonitorDigitalOutput(digitalOutputReceived);
        }

        /// <summary>
        /// Gets the digital input.
        /// </summary>
        /// <returns>the digital input.</returns>
        public uint GetDigitalInput()
        {
            uint statusBitmap = default(uint);
            Wrapper.GetDigitalInput(statusBitmap);

            return statusBitmap;
        }

        /// <summary>
        /// Gets the digital output.
        /// </summary>
        /// <returns>the digital output.</returns>
        public uint GetDigitalOutput()
        {
            uint statusBitmap = default(uint);
            Wrapper.GetDigitalOutput(statusBitmap);

            return statusBitmap;
        }

        /// <summary>
        /// Monitors the control status.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorControlStatus()
        {
            if (controlStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return Wrapper.WatchControl(controlStatusChanged);
        }

        /// <summary>
        /// Monitors the homing process.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorHomingStatus()
        {
            if (homingStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            var  result = Wrapper.WatchHoming(homingStatusChanged);
            return result;
        }

        /// <summary>
        /// Monitors the values for each joint on the robot.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorJointStatus()
        {
            if (jointStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return Wrapper.WatchJoint(jointStatusChanged);
        }

        #endregion


        #region Stop function

        //Stop
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ucGroup"></param>
        /// <returns></returns>
        public bool Stop(byte ucGroup)
        {
            return Wrapper.Stop(ucGroup);
        }

        #endregion

        #region Gripper Functions

        // OpenGripper
        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public bool OpenGripper()
        {
            return Wrapper.OpenGripper();
        }

        //CloseGripper
        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public bool CloseGripper()
        {
            return Wrapper.CloseGripper();
        }

        //GetJaw
        /// <summary>
        /// 
        /// </summary>
        /// <param name="percent"></param>
        /// <param name="mm"></param>
        /// <returns></returns>
        public bool GetJaw(ref short percent, ref short mm)
        {
            var result = Wrapper.GetJaw(ref percent, ref mm);
            return result;
        }

        #endregion

        #region Robot movement

        //DefineVector
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ucGroup"></param>
        /// <param name="szVectorName"></param>
        /// <param name="sDimension"></param>
        /// <returns></returns>
        public bool DefineVector(byte ucGroup, string szVectorName, short sDimension)
        {
            return Wrapper.DefineVector(ucGroup, szVectorName, sDimension);
        }

        public bool CloseTorque()
        {
            return Wrapper.CloseTorque();
        }

        /// <summary>
        /// Defines a vector in the robots memory.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="vectorName">Name of the vector.</param>
        /// <returns>
        /// a vector with the given vectorname; otherwise, null.
        /// </returns>
        public Vector DefineVector(Axis axis, string vectorName)
        {
            if (string.IsNullOrEmpty(vectorName))
            {
                throw new ArgumentException("vectorName cannot be null or empty.");
            }

            short dimension = 1;

            if (DefineVector((byte)axis, vectorName, dimension))
            {
                return new Vector(axis, vectorName, dimension);
            }
            else
            {
                return null;
            }
        }

        //Here
        /// <summary>
        /// 
        /// </summary>
        /// <param name="szVectorName"></param>
        /// <param name="sPointNumber"></param>
        /// <param name="lPointType"></param>
        /// <returns></returns>
        public bool Here(string szVectorName, short sPointNumber, long lPointType)
        {
            return Wrapper.Here(szVectorName, sPointNumber, lPointType);
        }

        /// <summary>
        /// Sets the joints.
        /// </summary>
        /// <param name="vector">The vector.</param>
        /// <param name="position">The position.</param>
        /// <param name="pointType">Type of the point.</param>
        /// <returns></returns>
        public bool SetJoints(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;
            

            return Wrapper.SetJoints(vector.Name, 1,
                position.Joint, (short)position.Joint.Length, (int)pointType);
        }

        //Teach
        /// <summary>
        /// 
        /// </summary>
        /// <param name="szVectorName"></param>
        /// <param name="sPointNumber"></param>
        /// <param name="plCoolArray"></param>
        /// <param name="sCoorArrayDim"></param>
        /// <param name="lPointType"></param>
        /// <returns></returns>
        public bool Teach(string szVectorName, short sPointNumber, int[] plCoolArray, short sCoorArrayDim, long lPointType)
        {
            return Wrapper.Teach(szVectorName, sPointNumber, plCoolArray, sCoorArrayDim, lPointType);
        }

        /// <summary>
        /// Teaches a robot the location of a named vector.
        /// </summary>
        /// <param name="vector">Vector to teach.</param>
        /// <param name="position">XYZ position to associate the vector with.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Teach(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;

            return Wrapper.Teach(vector.Name, 1,
                position.XYZ, (short)position.XYZ.Length, (int)pointType);
        }

        //MoveLinear
        /// <summary>
        /// 
        /// </summary>
        /// <param name="szVectorName"></param>
        /// <param name="sPointNumber"></param>
        /// <param name="szVectorNameB"></param>
        /// <param name="sPointNumberB"></param>
        /// <returns></returns>
        public bool MoveLinear(string szVectorName, short sPointNumber, string szVectorNameB, short sPointNumberB)
        {
            return Wrapper.MoveLinear(szVectorName, sPointNumber, szVectorNameB, sPointNumberB);
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveJoint(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return Wrapper.MoveJoint(vector.Name, vector.Dimension, null, 0);
            //return Wrapper.MoveLinear(vector.Name, vector.Dimension, null, 0);
        }

        public bool MoveTorque(byte axis, int torque)
        {
            return Wrapper.MoveTorque(axis, torque);
        }
        public bool MoveTorque(int[] plTorqueArray, short sTorqueArrayDim)
        {
            return Wrapper.MoveTorque(plTorqueArray, sTorqueArrayDim);
        }

        public void MoveToPosition(Position position, PointType pointType)
        {
            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            string vectorName = Guid.NewGuid().ToString("n");

            if (string.IsNullOrEmpty(vectorName))
            {
                throw new InvalidOperationException("Error when creating a GUID for the vectorName.");
            }

            var vector = DefineVector(Axis.Robot, vectorName);
            vector.Position = position;

            switch (pointType)
            {
                case PointType.RelativeJointCoordinate:
                case PointType.AbsoluteJointCoordinate:
                    if (!SetJoints(vector, vector.Position, pointType))
                    {
                        throw new NullReferenceException();
                    }
                    break;
                case PointType.AbsoluteXYZCoordinateA:
                case PointType.AbsoluteXYZCoordinateB:
                case PointType.RelativeXYZCoordinate:
                    if (!Teach(vector, vector.Position, pointType))
                    {
                       // throw new NullReferenceException();
                        Console.WriteLine("error in teaching");
                    }
                    if (!Wrapper.SavePoints("Points.txt"))
                    {
                       // throw new NullReferenceException();
                        Console.WriteLine("error in saving");
                    }
                    break;
            }

            MoveJoint(vector);
        }

        #endregion

        #region Status

        //IsOnlineOk
        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public bool IsOnlineOk()
        {
            return Wrapper.IsOnlineOk();
        }

        //GetCurrentPosition
        /// <summary>
        /// 
        /// </summary>
        /// <param name="pEnc"></param>
        /// <param name="pJoint"></param>
        /// <param name="pXYZ"></param>
        /// <returns></returns>
        public bool GetCurrentPosition(int[] pEnc, int[] pJoint, int[] pXYZ)
        {
            return Wrapper.GetCurrentPosition(pEnc, pJoint, pXYZ);
        }

        public Position GetCurrentPosition()
        {
            int[] enc = new int[USBC.TotalAxes];
            int[] joint = new int[USBC.TotalAxes];
            int[] xyz = new int[USBC.TotalAxes];

            Wrapper.GetCurrentPosition(enc, joint, xyz);

            return new Position()
            {
                Enc = enc,
                Joint = joint,
                XYZ = xyz
            };
        }

        public bool GetPointInfo(string szVectorName, short sPointNumber,
            int[] pEncData, int[] pXYZData, int[] lPointType)
        {
            return Wrapper.GetPointInfo(szVectorName, sPointNumber, pEncData, pXYZData, lPointType);
        }
        #endregion

        #region Manual movement

        //EnterManual
        /// <summary>
        /// 
        /// </summary>
        /// <param name="sManualType"></param>
        /// <returns></returns>
        public bool EnterManual(short sManualType)
        {
            return Wrapper.EnterManual(sManualType);
        }

        //CloseManual
        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public bool CloseManual()
        {
            return Wrapper.CloseManual();
        }

        //MoveManual
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ucAxis"></param>
        /// <param name="lVelocity"></param>
        /// <returns></returns>
        public bool MoveManual(byte ucAxis, long lVelocity)
        {
            return Wrapper.MoveManual(ucAxis, lVelocity);
        }

        #endregion

        #region Time and Speed

        //Time
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ucGroup"></param>
        /// <returns></returns>
        public bool Time(byte ucGroup, long lTime)
        {
            return Wrapper.Time(ucGroup, lTime);
        }

        //Speed
        /// <summary>
        /// 
        /// </summary>
        /// <param name="ucGroup"></param>
        /// <param name="lSpeed"></param>
        /// <returns></returns>
        public bool Speed(byte ucGroup, long lSpeed)
        {
            return Wrapper.Speed(ucGroup, lSpeed);
        }

        #endregion

        #endregion
    }
}
